#include <dalelib/graveyard/bone.h>

namespace dalelib
{

Bone::Bone(const string _Name,Bone* _Parent,const v3 _Start,const v3 _End,const v3 _Amin,const v3 _Amax)
	: _name(_Name)
	, _parent(_Parent)
	, _pos(0,0,0)
	, _ang(0,0,0)
	, _amin(_Amin)
	, _amax(_Amax)
{
	_length = (_End-_Start).len();
	_global_mat.lookAt(_Start,_End,v3(0,1,0));
	_global_mat_inv = _global_mat.inverse();

	if( _parent )
	{
		_local_mat = _parent->getGlobalMatInv()*_global_mat;
//		_local_mat = _global_mat*_parent->getGlobalMatInv();
	}
	else
	{
		_local_mat = _global_mat;
	}
	_local_mat_inv = _local_mat.inverse();
	_pos = _local_mat.getTranslationPart();

}

void Bone::update()
{
	_local_mat[12] = _pos[0]; _local_mat[13] = _pos[1]; _local_mat[14] = _pos[2]; // insert offset back into local mat
	m4 rot = m4::rotation(_ang);
	m4 temp_local_mat = _local_mat*rot;

	if( _parent )
		_global_mat = _parent->getGlobalMat()*temp_local_mat;
	else
		_global_mat = temp_local_mat;

//	_global_mat_inv = _global_mat.fastInverse();
	_global_mat_inv = _global_mat.inverse();

	_rest_to_pose_mat = _global_mat*_global_mat_rest_inv;
}

void Bone::drawUnitBone()
{
    static const float s = sqrt(2.f)*0.5f;
    static const GLfloat vertices[36] = {
         0.0, 0.0, 0.0,          0, 0,-1,
        -0.1,-0.1, 0.2,         -s,-s, 0,
         0.1,-0.1, 0.2,          s,-s, 0,
         0.1, 0.1, 0.2,          s, s, 0,
        -0.1, 0.1, 0.2,         -s, s, 0,
         0.0, 0.0, 1.0,          0, 0, 1,
    };
    static const GLuint indices[24] = {
        0,2,1,
        0,3,2,
        0,4,3,
        0,1,4,
        1,2,5,
        2,3,5,
        3,4,5,
        4,1,5,
    };

	glEnable(GL_DEPTH_TEST);

    glEnableClientState( GL_VERTEX_ARRAY );
    glVertexPointer( 3, GL_FLOAT, sizeof(GLfloat)*6, (GLvoid*)&vertices[0] );

	glEnableClientState( GL_NORMAL_ARRAY );
	glNormalPointer( GL_FLOAT, sizeof(GLfloat)*6, (GLvoid*)&vertices[3] );

    glDrawElements( GL_TRIANGLES, 24, GL_UNSIGNED_INT, (GLvoid*)&indices[0] );

    glDisableClientState( GL_VERTEX_ARRAY );
	glDisableClientState( GL_NORMAL_ARRAY );
}

void Bone::draw(const bool _IsCurrent)
{

	if( _parent )
	{
//*
        glLineWidth(1.f);
        glColor3f(0,0,1);
        glBegin(GL_LINES);
            v3 p1 = _global_mat.getTranslationPart();
            v3 p2 = _parent->getBoneEnd();
            glVertex3fv((float*)p1.ptr());
            glVertex3fv((float*)p2.ptr());
        glEnd();
	}
//*/
    glPushMatrix();
    glMultMatrixf( (float*)_global_mat.ptr() );
//            glLineWidth(1.f);
//            glDisable(GL_LIGHTING);
//            glDisable(GL_DEPTH_TEST);
//            drawTrihedron();
        glScalef(1,1,_length);
        v3 col(1,1,1);
        if(_IsCurrent) col = v3(1,0,0);
        glColor3fv((float*)col.ptr());
        glEnable(GL_LIGHTING);
        drawUnitBone();
    glPopMatrix();

/*
		glLineWidth(3.f);
		v3 col(0,0,0);
		if(_IsCurrent) col = v3(1,0,0);
		glColor3fv(col.ptr());
		glBegin(GL_LINES);
			glVertex3f(0,0,0);
			glVertex3f(0,0,_length);
		glEnd();
		glPopMatrix();
*/
}
/*
void Bone::dump()
{
	_pos.print("pos: ");
	_ang.print("ang: ");
}

*/


} // namespace dalelib
